import os
import sys
import yaml

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node

sys.path.append(os.path.join(get_package_share_directory('rm_vision_bringup'), 'launch'))


def generate_launch_description():

    from common import node_params, launch_params, robot_state_publisher, tracker_node
    from launch_ros.descriptions import ComposableNode
    from launch_ros.actions import ComposableNodeContainer, Node
    from launch.actions import TimerAction, Shutdown
    from launch import LaunchDescription

    nohardware_nodes = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('rm_vision_bringup'), 'launch'),
            '/no_hardware.launch.py']),
        )
    rs_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('realsense2_camera'), 'launch'),
            '/rs_launch.py']),
        launch_arguments={'json_file_path': os.path.join(
            get_package_share_directory('rm_vision_bringup'), 'config', 'cam_set.json' ), 'rgb_camera.profile': '848x480x60', 'rgb_camera.enable_auto_exposure': 'false', 'rgb_camera.auto_exposure_priority': 'false', 'rgb_camera.exposure': '30'}.items()
        # launch_arguments={}.items()
    )
    imu_nodes = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('fdilink_ahrs'), 'launch'),
            '/odom_tf.launch.py']),
        )
    
    
    

    # get robot_type.yaml and shoot_caliberate.yaml to serial_driver_node
    # robot_type_params = os.path.join(
    #     get_package_share_directory('rm_vision_bringup'), 'config', 'robot_type.yaml')
    # shoot_caliberate_params = os.path.join(
    #     get_package_share_directory('rm_vision_bringup'), 'config', 'shoot_caliberate.yaml')

    shoot_caliberate_params = yaml.safe_load(open(os.path.join(
        get_package_share_directory('rm_vision_bringup'), 'config', 'shoot_caliberate.yaml')))
    robot_type_params = yaml.safe_load(open(os.path.join(
        get_package_share_directory('rm_vision_bringup'), 'config', 'robot_type.yaml')))
    
    my_gun_calibs = shoot_caliberate_params[robot_type_params["robot_type"]]

    my_runtime = robot_type_params["running_on"]
    if my_runtime == "nx":
        print("Using on nx")
        nx_node = IncludeLaunchDescription(
            PythonLaunchDescriptionSource([os.path.join(
                get_package_share_directory('nx_processor'), 'launch'),
                '/launch.py']),
            )



    anti_spinner_node = Node(
        package='anti_spinner',
        executable='anti_spinner_node',
        name='anti_spinner',
        output='both',
        parameters=[my_gun_calibs],
        on_exit=Shutdown(),
        # ros_arguments=['--ros-args', '--log-level',
        #                'serial_driver:='+launch_params['serial_log_level']],
    )

    serial_driver_node = Node(
        package='rm_serial_driver',
        executable='rm_serial_driver_node',
        name='serial_driver',
        output='both',
        emulate_tty=True,
        parameters=[node_params],
        on_exit=Shutdown(),
        # ros_arguments=['--ros-args', '--log-level',
        #                'serial_driver:='+launch_params['serial_log_level']],
    )

    delay_serial_node = TimerAction(
        period=1.5,
        actions=[serial_driver_node],
    )


    should_run_node = [
        nohardware_nodes,rs_node,imu_nodes,
        delay_serial_node,anti_spinner_node,
    ]

    if my_runtime == "nx":
        should_run_node.append(nx_node)

    return LaunchDescription(should_run_node)